#include "Octree.h"

#include <cstdio>

Octree::Octree(const AABB& _aabb, int _depth)
: aabb(_aabb), depth(_depth)
{
	numTri = 0;
	internal = false;
	fprintf(stderr, "Octree() %.3lf %.3lf %.3lf %.3lf %.3lf %.3lf\n",
		aabb.corner1.x, aabb.corner1.y, aabb.corner1.z,
		aabb.corner2.x, aabb.corner2.y, aabb.corner2.z);
}

bool Octree::insert(Triangle* t)
{
	if(t == 0 || !aabb.intersect(t->aabb))
		return false;
	numTri++;
	//fprintf(stderr, "Octree::insert:2\n");
	if(internal) {
		for(int i = 0; i < 8; ++i) if(child[i]->aabb.intersect(t->aabb))
			child[i]->insert(t);
	} else {
		triangle.insert(t);
		fprintf(stderr, "Octree::insert3 really insert\n");
	}
	if(!internal && depth < maxOctreeDepth && numTri > maxTriPerOctree)
		split();
	return true;
}

bool Octree::erase(Triangle* t)
{
	return erase(t, t->oldAabb);
}

void Octree::update(Triangle* t)
{
	fprintf(stderr, "Octree::update\n");
	erase(t, t->oldAabb);
	insert(t);
}

void Octree::handleCollisions()
{
	if(internal) {
		for(int i = 0; i < 8; ++i)
			child[i]->handleCollisions();
		return;
	}
	// Compute a set of unique particles
	set<Particle*> particle;
	set<Particle*>::iterator p;
	set<Triangle*>::iterator t;
	for(t = triangle.begin(); t != triangle.end(); ++t) {
		particle.insert((*t)->p[0]);
		particle.insert((*t)->p[1]);
		particle.insert((*t)->p[2]);
	}
	// Collide each triangle with all particles
	for(t = triangle.begin(); t != triangle.end(); ++t) {
		for(p = particle.begin(); p != particle.end(); ++p) {
			(*t)->collide(*p);
		}
	}

	// Positions have changed, update octree
	for(t = triangle.begin(); t != triangle.end(); ++t) {
		(*t)->updateAABB();
		update(*t);
	}
}

Octree::~Octree()
{
	merge();
}

void Octree::merge()
{
	if(!internal)
		return;
	fprintf(stderr, "Octree::merge()\n");
	for(int i = 0; i < 8; ++i) {
		// Collect triangles
		child[i]->merge();
		set<Triangle*>::iterator t;
		for(t = child[i]->triangle.begin(); t != child[i]->triangle.end(); ++t)
			triangle.insert(*t);
		// Delete child
		delete child[i];
	}
	internal = false;
}

void Octree::split()
{
	fprintf(stderr, "Octree::split\n");
	if(internal)
		return; // node already has children
	// Create children
	Vector3 diag = aabb.center-aabb.corner1;
	for(int i = 0; i < 2; ++i) for(int j = 0; j < 2; ++j) for(int k = 0; k < 2; ++k) {
		Vector3 c1 = aabb.corner1 +
			i*Vector3(diag.x, 0, 0)+
			j*Vector3(0, diag.y, 0)+
			k*Vector3(0, 0, diag.z);
		child[i*4+j*2+k] = new Octree(AABB(c1, c1+diag), depth+1);
	}
	internal = true;
	// Split triangles
	set<Triangle*>::iterator t;
	for(t = triangle.begin(); t != triangle.end(); ++t) {
		fprintf(stderr, "distributing triangle\n");
		for(int i = 0; i < 8; ++i) if(child[i]->aabb.intersect((*t)->aabb))
			child[i]->insert(*t);
	}
	triangle.clear();
}

bool Octree::erase(Triangle* t, const AABB& tAabb)
{
	//fprintf(stderr, "Octree::erase\n");
	if(t == 0 || !aabb.intersect(tAabb))
		return false;
	//fprintf(stderr, "Octree::erase2\n");
	bool erased = false;
	if(internal) {
		for(int i = 0; i < 8; ++i) if(child[i]->aabb.intersect(tAabb)) {
			bool flag = child[i]->erase(t, tAabb);
			erased = erased || flag;
		}

	} else {
		erased = (triangle.find(t) != triangle.end());
		fprintf(stderr, "Octree::erase3 really erase\n");
		triangle.erase(t);
	}
	numTri -= erased;
	if(!internal && numTri < minTriPerOctree)
		merge();
	
	return erased;
}
